/**
 * Author: luoqi
 * Created Date: 2025-10-22 01:13:20
 * Last Modified: 2025-11-25 22:22:19
 * Modified By: luoqi at <**@****>
 * Copyright (c) 2025 <*****>
 * Description:
 */

#include <dev/xbot.h>
#include "cmds.hpp"

static QShell &cli = QShellObj::obj();
static PowerFlow &joints = XBotObj::obj().joints;

static int args_on(int argc, char **argv)
{
    if(argc != 2) {
        return -1;
    }
    int joint = strtol(argv[1], nullptr, 0);
    if((joint < 1) || (joint > joints.num())) {
        return -1;
    }

    int ret = joints.enable(joint);
    cli.print(" enable joint %d, ret: %d", joint, ret);
    return 0;
}

static int args_off(int argc, char **argv)
{
    if(argc != 2) {
        return -1;
    }
    int joint = strtol(argv[1], nullptr, 0);
    if((joint < 1) || (joint > joints.num())) {
        return -1;
    }
    int ret = joints.disable(joint);
    cli.print(" disable joint %d, ret: %d", joint, ret);
    return 0;
}

static int args_mit(int argc, char **argv)
{
    if(argc == 7) {
        int joint = strtod(argv[1], nullptr);
        if((joint < 1) || (joint > joints.num())) {
            return -1;
        }
        float pos = strtof(argv[2], nullptr);
        float vel = strtof(argv[3], nullptr);
        float tor = strtof(argv[4], nullptr);
        float kp = strtof(argv[5], nullptr);
        float kd = strtof(argv[6], nullptr);

        int ret = joints.mit_set(joint, pos, vel, tor, kp, kd);
        cli.print(" mit set joint %d, pos: %f, vel: %f, tor: %f, kp: %f, kd: %f, ret: %d",
            joint, pos, vel, tor, kp, kd, ret);
    } else {
        return -1;
    }

    return 0;
}

static QShell::ArgsTable table[] = {
    {"on", args_on, "<joint>, enable joint"},
    {"off", args_off, "<joint>, disable joint"},
    {"mit", args_mit, "[<joint> <pos> <vel> <tor> <kp> <kd>]- set mit mode control"},
};

int cmd_joints_hdl(int argc, char **argv)
{
    if(argc == 1) {
        for(auto &j : joints.info()) {
            cli.print(" id: 0x%04x, type: %d, mode: %d, pos: %f, vel: %f, cur: %f, err: %d, state: %d, hb: %d\r\n",
                j.id, j.type, j.mode, j.pos, j.vel, j.cur, j.err, j.state, j.hb);
        }
        return 0;
    }
    ARGS_TABLE_EXEC(cli, argc, argv, table);
}

EXPORT_CMD("joints", cmd_joints_hdl, "joints cmds");
